/*---------------------------------------------------------------------
 * File name: h3lis331dl.cpp
 *
 * Copyright (c) <2020-2022>, <ChenLong>
 *
 * All rights reserved.
 *
 * Author: ChenLong
 * Email: worldlong@foxmail.com
 *--------------------------------------------------------------------*/
#include "h3lis331dl.h"
#if defined(USING_RT_THREAD)
#include "rtthread.h" 
#endif
/*
*/
void H3LIS331DL::write_reg(uint8_t addr, uint8_t data)
{
#if defined(USING_RT_THREAD)
  rt_enter_critical();
#endif
  
  uint8_t tx[2]={(uint8_t)(addr&0x7f), data};
  uint8_t rx[2];
  for(int i=0; i<_ncs; i++) {
    _cs->write(0);
  }
  _spi->transfer(tx, rx, 2);
  
  for(int i=0; i<_ncs; i++) {
    _cs->write(1);
  }
 
#if defined(USING_RT_THREAD)
  rt_exit_critical();
#endif
}
/*
*/
uint8_t H3LIS331DL::read_reg(uint8_t addr)
{
#if defined(USING_RT_THREAD)
  rt_enter_critical();
#endif
  uint8_t tx[2]={(uint8_t)(addr|0x80), 0x00};
  uint8_t rx[2];
  for(int i=0; i<_ncs; i++) {
    _cs->write(0);
  }
  _spi->transfer(tx, rx, 2);
  
  for(int i=0; i<_ncs; i++) {
    _cs->write(1);
  }
  
#if defined(USING_RT_THREAD) 
  rt_exit_critical();
#endif
  return rx[1];
}
/*
*/
bool H3LIS331DL::write_reg_with_check(uint8_t addr, uint8_t data)
{
  write_reg(addr, data);
  return (data == read_reg(addr));
}
/*
number <= 8
*/
int H3LIS331DL::read_bytes(uint8_t *data, uint8_t addr, int number)
{
  if(number > 8) {
    number = 8;
  }
  
#if defined(USING_RT_THREAD)
  rt_enter_critical();
#endif
  uint8_t tx[9]={(uint8_t)(addr|0xc0), 0x00};
  uint8_t rx[9];

  for(int i=0; i<_ncs; i++) {
    _cs->write(0);
  }
  _spi->transfer(tx, rx, number + 1);
  
  for(int i=0; i<_ncs; i++) {
    _cs->write(1);
  }
  
  if(data) {
    memcpy(data, rx + 1, number);
  }
  
#if defined(USING_RT_THREAD) 
  rt_exit_critical();
#endif
  return number;
}
/*
*/
void H3LIS331DL::reset()
{
  write_reg(H3LIS331DL_CTRL_REG2, 0x80);
}
/*
*/
bool H3LIS331DL::check()
{
  return (read_reg(H3LIS331DL_WHO_AM_I) == H3LIS331DL_WHO_AM_I_DATA);
}
/*
*/
bool H3LIS331DL::config(uint8_t *cfg[2], int size)
{
  for(int i=0; i<size; i++) {
    if(!write_reg_with_check(cfg[i][0], cfg[i][1])) {
      return false;
    }
  }
  return true;
}
/*
*/
bool H3LIS331DL::config(int accel_fs, int odr)
{
  uint8_t odr_cfg;
  uint8_t fs_cfg;
  
  /* Config FS and ODR */
  if(odr == 50) {
    odr_cfg = 0 << 3;
  } else if(odr == 100) {
    odr_cfg = 1 << 3;
  } else if(odr == 400) {
    odr_cfg = 2 << 3;
  } else if(odr == 1000) {
    odr_cfg = 3 << 3;
  } else { //1000Hz
     odr_cfg = 3 << 3;
  }
  if(!write_reg_with_check(H3LIS331DL_CTRL_REG1, (1<<5)|odr_cfg|0x07)) {
    return false;
  }
  
  /* accel FS */
  if(accel_fs == 100) {
    scale = 0.049f;
    fs_cfg = 0 << 4;
  } else if(accel_fs == 200) {
    scale = 0.098f;
    fs_cfg = 1 << 4;
  } else if(accel_fs == 400) {
    scale = 0.195f; 
    fs_cfg = 3 << 4;
  } else { //400
    fs_cfg = 3 << 4;
  }
  if(!write_reg_with_check(H3LIS331DL_CTRL_REG4, fs_cfg)) {
    return false;
  }
  
  /* Interrupt config. Int1 and Int2 conneted to DRDY */
  if(!write_reg_with_check(H3LIS331DL_CTRL_REG3, 0x92)) {
    return false;
  }
  
  /* Int1 duration */
  if(!write_reg_with_check(H3LIS331DL_INT1_DURATION, 0x0f)) {
    return false;
  }
  
  /* Int2 duration */
  if(!write_reg_with_check(H3LIS331DL_INT2_DURATION, 0x0f)) {
    return false;
  }
  
  return true;
}
/*
*/
void H3LIS331DL::read_accel(int16_t *accel)
{
  uint8_t data[6];
  read_data(data);
  accel[0] = *((int16_t*)(&data[0])) / 16;
  accel[1] = *((int16_t*)(&data[2])) / 16;
  accel[2] = *((int16_t*)(&data[4])) / 16;
}
  
